import rclpy
from rclpy.node import Node
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import math
# import pupil_apriltags as apriltag
import apriltag
from sls_msgs.msg import SLSApirltag

class CameraDetect(Node):
    def __init__(self,source):
        super().__init__('apirltag_detmerge_node')
        self.pub = self.create_publisher(SLSApirltag,'apirltag_coordination',10)
        self.timer = self.create_timer(0.1,self.timer_callback)
        # self.detector = apriltag.Detector(families = 'tag36h11')
        self.detector = apriltag.Detector()
        self.cap = cv2.VideoCapture(source)
        if not self.cap.isOpened():
            self.get_logger().error('SLS: Failed to open camera, please ensure your camera source is 2!')
    def timer_callback(self):
        ret,frame = self.cap.read()
        id = -1
        isdetect =False
        if ret == True:
            gray_frame = cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)
        else:
            self.get_logger().error('SLS: convert false')
            return None
        try:
            results = self.detector.detect(gray_frame)
            if results != []:
                for result in results:
                    (cX, cY) = (int(result.center[0]), int(result.center[1]))
                    angle = int(math.atan((result.corners[0][1]-result.corners[3][1])/
                                        (result.corners[0][0]-result.corners[3][0]))*57.3)
                    id = result.tag_id
                    isdetect =True
                    if id == 10:
                        break
            else:
                isdetect =False
        except:
            isdetect =False
        # cv2.imshow("video", frame)
        # cv2.waitKey(1)
        coordinate = SLSApirltag()
        coordinate.isdetect = isdetect
        if isdetect == True:
            coordinate.x = cX
            coordinate.y = cY
            coordinate.angle = angle
            coordinate.id = id
        else:
            self.get_logger().warning('SLS: No coordination')
        self.pub.publish(coordinate)

            
def main(args=None):
    rclpy.init(args=args)
    node = CameraDetect(0)
    rclpy.spin(node)
    node.destroy_node(node)
    rclpy.shutdown()

if __name__ == "__main__":
    main()